/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Basketball;
import VISIE.characters.BasketballAgent;
import VISIE.characters.BasketballCharacter;
import VISIE.mathfunctions.Conversions;
import VISIE.models.BasketballPlayerModel;
import VISIE.scenemanager.Scene;
import VISIE.scenemanager.SceneCharacterManager;
import com.jme3.math.Vector3f;
import com.jme3.bullet.control.RigidBodyControl;
import com.jme3.scene.Node;
import java.util.ArrayList;
import VISIE.characters.Character;
import VISIE.characters.KinectPlayer;
import VISIE.characters.NonUserPlayer;
import VISIE.characters.Player;
import VISIE.scenemanager.Court;
import VISIE.scenemanager.SceneCreator;
import com.jme3.collision.CollisionResults;
import com.jme3.math.Quaternion;
import java.util.Random;

/**
 *
 * @author DiveshLala
 */
public class Ball {
    
    Node parentNode;
    RigidBodyControl ballPhysicsNode;
    Vector3f ballPosition;
    int playerInPossession;
    int lastTouched;
    float radius;
    
    public Ball(Vector3f initialPos, RigidBodyControl n, float r){
        
        ballPosition = initialPos;
        ballPhysicsNode = n;  
        radius = r;
        ballPhysicsNode.setFriction(0.5f);
        ballPhysicsNode.setRestitution(0.75f);
        ballPhysicsNode.setMass(0.65f);
        ballPhysicsNode.setCollisionGroup(2);
    }
    
    public void setMainNode(Node n){
        parentNode = n;
    }
    
    public Vector3f getBallPosition(){
        return ballPhysicsNode.getPhysicsLocation();
    }
    
    public void setBallPosition(Vector3f pos){
        ballPhysicsNode.setPhysicsLocation(pos);
    }
    
    public void setLastTouched(int i){
        lastTouched = i;
    }
    
    public int getLastTouched(){
        return lastTouched;
    }
     
    public void removeDamping(){
        ballPhysicsNode.setDamping(0, 0);
    }
    
    public void updateBallInPossession(){
         Vector3f vec = SceneCharacterManager.getCharacterByID(playerInPossession).getHandPosition(1);
         vec.subtractLocal(0, radius, 0);
  //       this.setBallPosition(vec); 
         Character c = SceneCharacterManager.getCharacterByID(playerInPossession);
         
        if(c instanceof VISIE.characters.KinectPlayer){
            if(this.isCloseToPlayer((KinectPlayer)c)){
                this.adjustCollisionGroup(true);
            }
            else{
                this.adjustCollisionGroup(false);
            }
            
            KinectPlayer kp = (KinectPlayer)c;
            if(!kp.isDribbling()){
                this.setBallPosition(vec);
            }
        }
        else if(c instanceof VISIE.characters.NonUserPlayer){
            if(this.isCloseToNUP((NonUserPlayer)c)){
                this.adjustCollisionGroup(true);
            }
            else{
                this.adjustCollisionGroup(false);
            }
            
            NonUserPlayer nup = (NonUserPlayer)c;
            if(nup.getKinectNUP() && nup.isDribbling()){
            }
            else{
                this.setBallPosition(vec); 
            }
        } 
        else{
            this.setBallPosition(vec); 
        }
    }
    
    public void updateBallWithoutPossession(){
        
      //  this.removeSpin();
        this.reduceSpin();
        this.adjustCollisionGroup(false);
        ArrayList<Character> clist = SceneCharacterManager.getCharacterArray();
        
        for(int i = 0; i < clist.size(); i++){
            
            Character c = clist.get(i);
            CollisionResults results = new CollisionResults();
            parentNode.collideWith(c.getCharacterMesh().getWorldBound(), results);
             
            if(c instanceof VISIE.characters.KinectPlayer){
                if(this.isCloseToPlayer((KinectPlayer)c)){
                    this.adjustCollisionGroup(true);
                }
                else{
                    this.adjustCollisionGroup(false);
                }
            }
            else if(c instanceof VISIE.characters.NonUserPlayer){
                if(this.isCloseToNUP((NonUserPlayer)c)){
                    this.adjustCollisionGroup(true);
                }
                else{
                    this.adjustCollisionGroup(false);
                }
            }
            
            if(results.size() > 0){
                BasketballCharacter bc = (BasketballCharacter)c;
                    if(!bc.isShooting() && !bc.isPassing() && bc.canSeeBall(this)){
                        bc.setPossession();
                        bc.setActionState(0);
                        if(bc.getCharacterType().equals("BasketballAgent")){
                            BasketballAgent ba = (BasketballAgent)bc;
                            ba.setBehaviorState(0);
                        }
                        else if(bc instanceof VISIE.characters.KinectPlayer){
                            this.stopBall();
                        }
                        System.out.println(playerInPossession + " is now in possession");   
                    }
            }
        }
    }
    
    public void setPossession(int i){
        playerInPossession = i;
        
        if(SceneCharacterManager.getCharacterByID(playerInPossession) instanceof VISIE.characters.Player){
             this.adjustCollisionGroup(true);
         }
         else{
             this.adjustCollisionGroup(false);
         }
    }
    
    public void passBall(float angle){
        float yForce = 20;
        this.applyForceToBall(angle, yForce);
        this.reduceSpin();
   //     playerInPossession = -1;
    }
    
    public void passBall(Character passer, Character receiver){
        Vector3f ballPos = this.getBallPosition();
        Vector3f target = receiver.getPosition();
        
     //   float angle = Conversions.originToTargetAngle(passer.getPosition(), receiver.getPosition());
    //    this.passBall(angle);
        float releaseAngle = (float)Math.toRadians(20);
      
        float h = target.y - ballPos.y;
        double l = Math.sqrt(Math.pow(target.x - ballPos.x, 2) + Math.pow(target.z - ballPos.z, 2));
        double a = l/Math.cos(releaseAngle);
        double b = 9.8/(2 * ((l * Math.tan(releaseAngle)) - h));
        double initialVelocity = a * Math.sqrt(b);
        Vector3f targetVector = Conversions.degreesToNormalizedCoordinates(Conversions.originToTargetAngle(this.getBallPosition(), target));

        float heightForce = (float)initialVelocity * (float)Math.sin(releaseAngle);
       float lengthForce = (float)initialVelocity * (float)Math.cos(releaseAngle);  
//        
        Vector3f forceVector = new Vector3f(targetVector.x * lengthForce, heightForce, targetVector.z * lengthForce);
//        
  //      if(Vector3f.isValidVector(forceVector)){
            this.applyForceToBall(forceVector);
            this.reduceSpin();
  //          return true;
 //       }
//        else{
//            return false;
//        }
    }
    
    
    
    public void bounceBall(Vector3f movementVector){
        Vector3f yForce = Vector3f.UNIT_Y.mult(-15f);
        this.applyForceToBall(movementVector.add(yForce));
    }
    
    public boolean shootBall(){
        Vector3f ballPos = this.getBallPosition();
        Vector3f goalPos = new Vector3f(Court.getHoopLocation());
        Vector3f goal2D = new Vector3f(Court.getHoopLocation().x, 0, Court.getHoopLocation().z);
        float releaseAngle;
        
        if(ballPos.distance(goal2D) < 20){
            releaseAngle = (float)Math.toRadians(70);
        }
        else{
            releaseAngle = (float)Math.toRadians(50);
        }
        
        float h = goalPos.y - ballPos.y;
        double l = Math.sqrt(Math.pow(goalPos.x - ballPos.x, 2) + Math.pow(goalPos.z - ballPos.z, 2));
        double a = l/Math.cos(releaseAngle);
        double b = 9.8/(2 * ((l * Math.tan(releaseAngle)) - h));
        double initialVelocity = a * Math.sqrt(b);
        Vector3f targetVector = Conversions.degreesToNormalizedCoordinates(Conversions.originToTargetAngle(this.getBallPosition(), goalPos));

        float heightForce = (float)initialVelocity * (float)Math.sin(releaseAngle);
        float lengthForce = (float)initialVelocity * (float)Math.cos(releaseAngle);  
        
        
        Vector3f forceVector = new Vector3f(targetVector.x * lengthForce, heightForce, targetVector.z * lengthForce);
        Vector3f modVec = this.calculateShootingError(forceVector);
        
        
        if(Vector3f.isValidVector(forceVector)){
            System.out.println(forceVector);
            System.out.println(modVec);
            this.applyForceToBall(modVec);
            this.reduceSpin();
        //    this.removeSpin();
       //     this.removeDamping();
            return true;
        }
        else{
            return false;
        }

    }
    
    private Vector3f calculateShootingError(Vector3f idealVec){
        float xError = 0;
        float xMult = 0;
        float yError = 0;
        float yMult = 0;
        float zError = 0;
        float zMult = 0;
        float angleToGoal = Conversions.originToTargetAngle(this.getBallPosition(), Court.getHoopLocation());
        float distanceToGoal = this.getBallPosition().distance(Court.getHoopLocation());
        float facingDirection = SceneCharacterManager.getCharacterByID(playerInPossession).getFacingDirection();
        float angleDiff = Math.abs(angleToGoal - facingDirection);
        float shotError = 0;
        float distError = 0;
    //    System.out.println("angle diff " + angleDiff + "dist to goal " + distanceToGoal);
        
        if(angleDiff < 2f){
            shotError += 0;
        }
        else if(angleDiff < 10f){
            shotError += 1f;
        }
        else{
            shotError += 3f;
        }
        
        if(distanceToGoal < 10f){
            distError += 0; 
        }
        else if(distanceToGoal < 20f){
            distError += 1f;
        }
        else{
            distError += 2f;
        }
        
        //0.03 for max mult
        xMult = idealVec.x * (0.03f * shotError);
        yMult = idealVec.y * (0.03f * distError);
        zMult = idealVec.z * (0.03f * shotError);
        
        Random r = new Random();
        
        xError = (float)r.nextGaussian() * xMult; 
        yError = (float)r.nextGaussian() * yMult;
        zError = (float)r.nextGaussian() * zMult;
        
 //       System.out.println("mults " + xMult + " " + yMult + " " + zMult);
  //      System.out.println("errors " + xError + " " + yError + " " + zError);
        
        return new Vector3f(idealVec.x + xError, idealVec.y + yError, idealVec.z + zError);
    }
    
    private void applyForceToBall(float angle, float yForce){
        ballPhysicsNode.setLinearVelocity(Conversions.degreesToNormalizedCoordinates(angle).mult(yForce).add(0, 5, 0));
    }
    
    private void applyForceToBall(Vector3f force){
        ballPhysicsNode.setLinearVelocity(force);
    }
    
    public boolean isBallInSpace(Vector3f pos){
        Vector3f ballPos = this.getBallPosition();
        boolean x = ((Math.pow(ballPos.x - pos.x, 2) + Math.pow(ballPos.y - pos.y, 2) + Math.pow(ballPos.z - pos.z, 2)) <= radius);
        return x;
    }
    
    public void removeSpin(){
        ballPhysicsNode.setPhysicsRotation(Quaternion.ZERO);
        ballPhysicsNode.setAngularFactor(0);
        ballPhysicsNode.setDamping(0.0f, 0f);
    }
    
    public void reduceSpin(){
        ballPhysicsNode.setAngularFactor(0.02f);
        this.removeDamping();
    }
    
    public void adjustCollisionGroup(boolean isTurnedOn){

        if(isTurnedOn){
            ballPhysicsNode.addCollideWithGroup(3);
            ballPhysicsNode.removeCollideWithGroup(1);
//            System.out.println(ballPhysicsNode.getLinearVelocity());
//            System.out.println("cg " + ballPhysicsNode.getCollisionGroup());
//            System.out.println("cwg " + ballPhysicsNode.getCollideWithGroups());

        }
        else{
            ballPhysicsNode.addCollideWithGroup(1);
            ballPhysicsNode.addCollideWithGroup(2);
//            System.out.println(ballPhysicsNode.getLinearVelocity());
//            System.out.println("cg " + ballPhysicsNode.getCollisionGroup());
//            System.out.println("cwg " + ballPhysicsNode.getCollideWithGroups());
        }
    }
    
    public boolean isBouncingUp(){
       return ballPhysicsNode.getLinearVelocity().getY() > 0;
    
    }
    
    public void stopBall(){
        ballPhysicsNode.setLinearVelocity(Vector3f.ZERO);
        ballPhysicsNode.setAngularVelocity(Vector3f.ZERO);
        this.removeDamping();
        this.removeSpin();
    }
    
    private boolean isCloseToPlayer(Player kp){
        
        for(int i = 0; i < kp.getGhostNode().getOverlappingObjects().size(); i++){
            Node n = (Node)kp.getGhostNode().getOverlappingObjects().get(i).getUserObject();
            if(n.getName().equals("Ball")){
                return true;
            }   
        }
        
        return false;       
    }
    
    private boolean isCloseToNUP(NonUserPlayer nup){
        
        for(int i = 0; i < nup.getGhostNode().getOverlappingObjects().size(); i++){
            Node n = (Node)nup.getGhostNode().getOverlappingObjects().get(i).getUserObject();
            if(n.getName().equals("Ball")){
                return true;
            }   
        }
        
        return false;       
    }
    
    public Vector3f getBallTravellingDirection(){
        return ballPhysicsNode.getLinearVelocity();
    }
    
}
